#include "bootloader.h"
#include "string.h"
#include "crc32.h"
#include "stm32l0xx_nucleo.h"

typedef  void (*func_ptr)(void);

typedef __packed struct {
    uint32_t crc;
    uint32_t len;
    uint32_t data[];
} ota_t;

/*private prototype function***********************************************************************/

void bootloader_check_firmware_update(void)
{
    FLASH_EraseInitTypeDef EraseInitStruct;
    ota_t* ota = (ota_t*) OTA_ADDR;
    uint32_t ota_crc, i, pages;
    uint32_t app_addr;
    uint32_t page_error;
    func_ptr app_reset;
    
    /*no OTA image, boot directly*/
    if (ota->len >= PAGE_SIZE * MAX_PAGES){
        goto boot;
    }

    /* same image, boot directly */
    if (memcmp((uint8_t*)LOAD_ADDR, (uint8_t*)(OTA_ADDR + 8), ota->len) == 0){
        goto boot;
    }

    /*check if OTA image is good*/
    ota_crc = crc32((uint8_t*) (OTA_ADDR + 4), ota->len + 4);
    if (ota_crc != ota->crc){
        goto boot;
    }
    
    /*now perform upgrade*/
    HAL_FLASH_Unlock();
    /*clear  write proctect flag*/
    __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_WRPERR);
    /*erase old image first*/
    pages = (ota->len + PAGE_SIZE - 1) / PAGE_SIZE;
    if (pages > MAX_PAGES) pages = MAX_PAGES;
    
    EraseInitStruct.TypeErase = FLASH_TYPEERASE_PAGES;
    EraseInitStruct.PageAddress = LOAD_ADDR;
    EraseInitStruct.NbPages = pages;
     
    HAL_FLASHEx_Erase(&EraseInitStruct, &page_error);

    /*program new image*/
    for (i = 0; i < (ota->len)/4; i++) {
        HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, LOAD_ADDR + i * 4, ota->data[i]);
    }
    HAL_FLASH_Lock();
    
    boot:
    /* Vector Table Relocation in Internal FLASH. */
    SCB->VTOR = LOAD_ADDR;
    app_addr = *(__IO uint32_t*) (LOAD_ADDR + 4);
    app_reset = (func_ptr) app_addr;
    __set_MSP(*(__IO uint32_t*) LOAD_ADDR);
    app_reset();
        
}
   

